#!/usr/bin/env python
import roslib; roslib.load_manifest('faces')
import rospy
import message_filters

from geometry_msgs.msg import Point
from sensor_msgs.msg   import CameraInfo,PointCloud
from people_msgs.msg   import PositionMeasurement
from faces.msg         import Face,Faces
from sensor_msgs.msg   import Image, CameraInfo

class FaceDetector:
    assumedFaceHeight = 0.1524
    assumedFaceWidth  = 0.0762

    def __init__(self):
        self.image = Image()
        rospy.Subscriber('/faces/image', Image, self.recordImage)
        rospy.Subscriber('/faces/cloud', PointCloud, self.recordCloud)
        self.publisher = rospy.Publisher('/faces/data', Faces)


    def translateFacePointsToFacesMessage(self, facePoints, image):
        faces = Faces()
        faces.header = facePoints.header
        for facePoint in facePoints.points:
            upperLeft = Point(
                facePoint.x-self.assumedFaceWidth/2.0,
                facePoint.y,
                facePoint.z+self.assumedFaceHeight/2.0
            )
            lowerRight = Point(
                facePoint.x+self.assumedFaceWidth/2.0,
                facePoint.y,
                facePoint.z-self.assumedFaceHeight/2.0
            )
            face            = Face()
            face.upperLeft  = upperLeft
            face.lowerRight = lowerRight
            face.center     = facePoint
            faces.faces.append(face)
            faces.image = image

        return faces

    def recordImage(self, image):
        self.image = image

    def recordCloud(self, pointCloud):
        self.handleMessages(pointCloud, self.image)

    def handleMessages(self, pointCloud, image):
        rospy.loginfo('handling message')
        faces = self.translateFacePointsToFacesMessage(pointCloud, image)
        self.publisher.publish(faces)

def main():
    rospy.init_node('face_detector')
    detector = FaceDetector()
    rospy.spin()

if __name__ == '__main__':
    main()
